


//#include "src/hexapod/debug.h"
#include "normal_mode.h"
#include "BTSerial.h"
#include "setting_mode.h"

#include "hexapod.h"
#include "pwm.h"
#include "ps2.h"
#include "PS3_contrl.h"
#include <soc/soc.h> //disable brownour problems
#include <soc/rtc_cntl_reg.h> //disable brownour problems
#define LED_BUILTIN 0
#define PS_BL_SECLECT 32
//
//  mode
//    0:  normal mode
//    1:  setting mode (for servo calibration)
//
char ps3_enable=1;
static int _mode = 0;
using namespace hexapod::hal;
//PCA9685 pwm(0x41);
static void log_output(const char* log) {
  Serial.println(log);
}

//
// wait for N secs, enter setting mode if pressed USR button
//
void boot_wait(int wait_for = 2000) {
  digitalWrite(LED_BUILTIN, HIGH);

  pinMode(6, INPUT);
  while (wait_for > 0) {
    if (HIGH == digitalRead(6)) {
      _mode = 1;
      break;
    }
    delay(100);
    wait_for -= 100;
  }

  digitalWrite(LED_BUILTIN, LOW);
}

//
//  Do common setup and mode-specific setup
//
void setup() {
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
//  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
  //Initialize serial and wait for port to open:
  Serial.begin(115200);
  Serial.println("Starting...");
   pinMode(PS_BL_SECLECT, INPUT_PULLUP);
   ps3_enable=digitalRead(PS_BL_SECLECT);
   Serial.printf("ps3_enable is %d \r\n",ps3_enable);
//ps3_enable=0;
   if(ps3_enable==1) ps3_init();else init_serial_BT();
   
  //
   //Common setup()
  //
 // while(1)

  init_ps2();
  	
  // Initialize GPIO
  //pinMode(LED_BUILTIN, OUTPUT);

  // Workaround for beta PCB
  //pinMode(2, INPUT);
  //pinMode(3, INPUT);

 // boot_wait();

//  hexapod::initLogOutput(log_output, millis);
 

  hexapod::Hexapod.init(_mode == 1);
 

  //
  // Mode-specific setup()
  //

  if (_mode == 0) {
 //   normal_setup();
  }
  else if (_mode == 1) {
 //   setting_setup();
  }

  Serial.print("Started, mode=");
  Serial.println(_mode);
  //pwm.begin();
}

void loop() {
Serial.println(digitalRead(17));
    normal_loop();

}
